import lejos.nxt.*;
import lejos.robotics.subsumption.Behavior;

/**
 * Make sure the robot stays within the boundaries of the field.
 */
public class StayInField implements Behavior {
	private boolean suppressed = false;
	private LightSensorValue tape;
	private int turnRate = 150;
	private int turnBackRate = 240;

	/**
	 * Default constructor.
	 */
	public StayInField() {
		tape = new LightSensorValue(40, 55);
	}

	private boolean weHadGreen;

	/**
	 * Make a turn when we take action.
	 */
	public void action() {
		suppressed = false;

		simpleTurn();
	}

	/**
	 * Simple turn by x degrees. Tries to make shortest turn.
	 */
	private void simpleTurn() {
		float compass = BillManager.getCompassData();
		boolean turnRight;

		if (compass > 180) {
			turnRight = false;
		} else {
			turnRight = true;
		}

		while (!suppressed && tape.InRange(BillManager.GetLightData())) {
			if (turnRight) {
				if (!Motor.B.isMoving()) {
					Motor.B.forward();
				}
				// Motor.A.backward();
			} else {
				if (!Motor.A.isMoving()) {
					Motor.A.forward();
				}
				// Motor.B.backward();
			}
		}

		Motor.A.stop();
		Motor.B.stop();
	}

	/**
	 * Makes the longest turn possible until we hit the line again. When we hit
	 * the line, turn back a little.
	 */
	@SuppressWarnings("unused")
	private void turnEntireField() {
		weHadGreen = false;

		float compass = BillManager.getCompassData();
		boolean turnLeft;

		if (compass > 180) {
			turnLeft = false;
		} else {
			turnLeft = true;
		}

		boolean exitLoop = false;
		while (!suppressed && !exitLoop) {
			if (turnLeft) {
				Motor.B.forward();
				// Motor.A.backward();
			} else {
				Motor.A.forward();
				// Motor.B.backward();
			}

			if (tape.InRange(BillManager.GetLightData()) && weHadGreen) {
				Motor.A.stop();
				Motor.B.stop();

				if (turnLeft) {
					Motor.A.rotate(turnBackRate, true);
					Motor.B.rotate(-turnBackRate, false);
				} else {
					Motor.B.rotate(turnBackRate, true);
					Motor.A.rotate(-turnBackRate, false);
				}
				exitLoop = true;
			}

			if (!tape.InRange(BillManager.GetLightData())) {
				weHadGreen = true;
			}
		}

		Motor.A.stop();
		Motor.B.stop();
	}

	/*
	 * (non-Javadoc)
	 * 
	 * @see lejos.robotics.subsumption.Behavior#suppress()
	 */
	public void suppress() {
		suppressed = true;
	}

	/**
	 * Take control if we the light data is within given range.
	 */
	public boolean takeControl() {
		int value = BillManager.GetLightData();

		if (tape.InRange(value)) {
			return true;
		}

		return false;
	}

	/**
	 * @author Mark Internal class to be able to assign areas based on color.
	 */
	private class LightSensorValue {
		private int lowerBound;
		private int upperBound;

		/**
		 * Constructor.
		 * 
		 * @param lower
		 *            Lower light data.
		 * @param upper
		 *            Higher light data.
		 */
		public LightSensorValue(int lower, int upper) {
			this.lowerBound = lower;
			this.upperBound = upper;
		}

		/**
		 * Gets the lower bound.
		 * 
		 * @return Lower bound.
		 */
		public int GetLowerBound() {
			return lowerBound;
		}

		/**
		 * Gets the higher bound.
		 * 
		 * @return Higher bound.
		 */
		public int GetUpperBound() {
			return upperBound;
		}

		/**
		 * @param value
		 * @return
		 */
		public boolean InRange(int value) {
			if (value > lowerBound && value < upperBound)
				return true;
			return false;
		}
	}
}
